% PositionSov.m

function res = PositionSov(inputTheta, initTheta)
% inputTheta 杆1和杆4的角度
% initTheta 用于解算杆2和杆3的初始角度
% res 输出c点坐标[xc xy]
    load("ConstantDefinition.mat", "l0", "theta0", "l1", "l2", "l3", "l4");
    theta1 = inputTheta(1);
    theta2_0 = initTheta(1);
    theta3_0 = initTheta(2);
    theta4 = inputTheta(2);

    opt=optimset('Display','off');    % 取消输出

    fun = @(x)Five_Link([l0, l1, l2, l3, l4], [theta0, theta1, x(1), x(2), theta4]);
    [y,~,exitflag,output,~] = fsolve(fun, [theta2_0, theta3_0], opt);
    if exitflag <= 0
        res=[99999,99999];
        disp('fsolve failed');
        return;
    end
    theta2 = y(1);
    theta3 = y(2);
    res(1) = l1*cos(theta1) + l2*cos(theta2);
    res(2) = l1*sin(theta1) + l2*sin(theta2);
    res(3) = y(1);
    res(4) = y(2);
end

function res = Five_Link(l, theta)
    l0 = l(1);
    l1 = l(2);
    l2 = l(3);
    l3 = l(4);
    l4 = l(5);
    theta0 = theta(1);
    theta1 = theta(2);
    theta2 = theta(3);
    theta3 = theta(4);
    theta4 = theta(5);
    res(1) = -l0*cos(theta0) + l1*cos(theta1) + l2*cos(theta2) - l3*cos(theta3) - l4*cos(theta4);
    res(2) = -l0*sin(theta0) + l1*sin(theta1) + l2*sin(theta2) - l3*sin(theta3) - l4*sin(theta4);
end